/* ######################################################################### */
/** \file KalmanObjectIdentification.hpp
 * \brief       This file contains the function that
 *
 *
 * PROJECT:   DATMO ROS NODE
 *
 * Copyright (c) 2011 CAR - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodriguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2011-06-23
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ         2011-06-23 \n
 *      First .
 *
 ######################################################################### */


#include <math.h>
#include <set>
/* ------------------------------------------------------------------------- */
/**     \namespace DATMO
 *
 *  \brief      This is the namespace of the DATMO ROS NODE
 */
/* ------------------------------------------------------------------------- */

namespace DATMO
{


class cl_KalAuxFun{
public:


  static ReturnMatrix EliminateRow(const Matrix& A,int i_row)
  {
    //MIRAR ESTO MAÑANA
    //http://www.robertnz.net/nm10.htm#memory
    Matrix C = Matrix(A.Nrows()-1,A.Ncols());
    C.SubMatrix(1,i_row-1,1,C.Ncols()) = A.SubMatrix(1,i_row-1,1,A.Ncols());
    C.SubMatrix(i_row,C.Nrows(),1,C.Ncols()) = A.SubMatrix(i_row+1,A.Nrows(),1,A.Ncols());
    C.Release();
    return C;
  }

  static  ReturnMatrix EliminateCol(const Matrix& A,int i_col)
  {
    //MIRAR ESTO MAÑANA
    //http://www.robertnz.net/nm10.htm#memory
    Matrix C = Matrix(A.Nrows(),A.Ncols()-1);
    C.SubMatrix(1,C.Nrows(),1,i_col-1) = A.SubMatrix(1,A.Nrows(),1,i_col-1);
    C.SubMatrix(1,C.Nrows(),i_col,C.Ncols()) = A.SubMatrix(1,A.Nrows(),i_col+1,A.Ncols());
    C.Release();
    return C;
  }

  static ReturnMatrix InsertMatrix(const Matrix& A,const Matrix& B, int i_row,  int i_col)
  {
    Matrix C = Matrix(A.Nrows()+B.Nrows(), A.Ncols()+B.Ncols());
    C = 0.0;
    if (A.Nrows()>0 && A.Ncols()>0)
    {
      if (i_row > 1 && i_col > 1)
      {
        C.SubMatrix(1,i_row-1,1,i_col-1) = A.SubMatrix(1,i_row-1,1,i_col-1);
        C.SubMatrix(i_row,i_row+B.Nrows()-1,i_col,i_col+B.Ncols()-1) = B;
        C.SubMatrix(i_row+B.Nrows(),C.Nrows(),i_col+B.Ncols(),C.Ncols()) = A.SubMatrix(i_row,A.Nrows(),i_col,A.Ncols());
      }
      if (i_row == 1 && i_col == 1)
      {
        C.SubMatrix(1,B.Nrows(),1,B.Ncols()) = B;
        C.SubMatrix(B.Nrows()+1,C.Nrows(),B.Ncols()+1,C.Ncols()) = A;
      }
      if (i_row == 1 && i_col>1)
      {
        C.SubMatrix(1,B.Nrows(),i_col,i_col+B.Ncols()-1) = B;
        C.SubMatrix(B.Nrows()+1,C.Nrows(),1,i_col-1) = A.SubMatrix(1,A.Nrows(),1,i_col-1);
        C.SubMatrix(B.Nrows()+1,C.Nrows(),i_col+B.Ncols(),C.Ncols()) = A.SubMatrix(1,A.Nrows(),i_col,A.Ncols());
      }
      if (i_row > 1 && i_col==1)
      {
        C.SubMatrix(i_row,i_row+B.Nrows()-1,1,B.Ncols()) = B;
        C.SubMatrix(1,i_row-1,B.Ncols()+1,C.Ncols()) = A.SubMatrix(1,i_row-1,1,A.Ncols());
        C.SubMatrix(i_row+B.Nrows(),C.Nrows(),B.Ncols()+1,C.Ncols()) = A.SubMatrix(i_row,A.Nrows(),1,A.Ncols());
      }
    }
    else
    {
      C = B;
    }
    C.Release();
    return C;

  }

  static ReturnMatrix AddRow(const ColumnVector& A,float value)
  {
    int nrows = A.Nrows();
    ColumnVector X(nrows+1);
    X.Rows(1,nrows) = A;
    X(nrows+1) = value;
    X.Release();
    return X;
  }

  static ReturnMatrix AddRow(const ColumnVector& A,const ColumnVector& B)
  {
    int nrows = A.Nrows();
    ColumnVector X(nrows+B.Nrows());
    X.Rows(1,nrows) = A;
    X.Rows(nrows+1,nrows+B.Nrows()) = B;
    X.Release();
    return X;
  }

};

/* ------------------------------------------------------------------------- */
/**     \class ObjecIdentification
 *
 *  \brief  This is the class of Objects2Real function.
 *
 *
 */
/* ------------------------------------------------------------------------- */
class cl_KalmanObjectIdentification
{
public:
  //Kalman Filter constants
  bool _first;
  double _INC_TIME;
  double _cP_Pos;
  double _cP_Size;
  double _cP_Color;
  double _cP_Temp;

  double _cQ_Pos;
  double _cQ_Size;
  double _cQ_Color;
  double _cQ_Temp;

  double _cR_Pos;
  double _cR_Size;
  double _cR_Color;
  double _cR_Temp;

  static const int _N_VAR = 8; //Number of variables in each object state
  static const int _N_U = 3;   //Number of entrances measurements


  //Kalman Filter Variables
  ColumnVector _Xk;
  ColumnVector _Xk_ant;
  ColumnVector _Xk_ant2;
  Matrix _Pk ;
  ColumnVector _Zk ;          // Matrix with all the measurement vectors (nvar rows * n detected objects columns)
  ColumnVector _Xpred ;
  Matrix _Ppred ;
  ColumnVector _Xk_sub ;
  Matrix _Pk_sub ;
  ColumnVector _Xpred_sub;   //part of the X predicted
  Matrix _Ppred_sub ;   //part of the P predicted
  ColumnVector _Zk_sub ;       //part of the measurement vector

  ColumnVector _TimeDetection ; //Time since detection
  ColumnVector _ObjectID ; //Object ID
  double _maxID ;
  float _LastTime;

  int _N_OBJ;


  ColumnVector _ObjNotDetected;
  ColumnVector _NewObjDetected ;

  //Equation variables
  Matrix _Ak;
  Matrix _Bk ;
  Matrix _Hk_sub ;
  ColumnVector _Uk ;
  Matrix _Yk_sub ;
  Matrix _Sk_sub ;
  Matrix _Kk_sub ;


  Matrix _Qk;
  Matrix _Rk ;
  Matrix _Rk_sub;


  //Colors
  /*
      black     = 0,RGB: 0,0,0
  grey      = 1,RGB: 190,190,190
     blue      = 2,RGB: 0,0,255
    green     = 3, RGB:0,255,0
   red       = 4, RGB:255,0,0
    yellow    = 5, RGB:255,255,0
      white     = 6 ,RG: 255,255,0
   */
  //Kalman Filter Functions
  cl_KalmanObjectIdentification()
  {
    _first = true;
    _INC_TIME = 0.250;
    _cP_Pos = 0.01;
    _cP_Size = 0.01;
    _cP_Color = 0.01;
    _cP_Temp = 0.01;

    _cQ_Pos = 0.1;
    _cQ_Size = 0.1;
    _cQ_Color = 0.1;
    _cQ_Temp = 0.1;

    _cR_Pos = 0.1;
    _cR_Size = 0.1;
    _cR_Color = 0.1;
    _cR_Temp = 0.1;

    //   _N_VAR = 8; //Number of variables in each object state
    //    _N_U = 2;

    _maxID = 0;
    _LastTime = 0.0f;


    _N_OBJ = 0;
  }

  /* ------------------------------------------------------------------------- */
  /**     \fn public
   *
   *  \brief
   *
   *  \param[in]
   *
   *  \param[out]
   *
   */
  /* ------------------------------------------------------------------------- */
  void InitializeFilter(cl_DynamicObjects DynObjs, float RobotID)
  {

    int n_states = _N_VAR * DynObjs.List.size();
    int n_aux1 = 1;
    int n_aux2 = 0;
    bool first = true;
    int DynObjSize = DynObjs.List.size();
    _N_OBJ = DynObjs.List.size();
    _Xk = ColumnVector(n_states);
    _TimeDetection = ColumnVector(DynObjSize);
    _TimeDetection = 0.0;
    _ObjectID = ColumnVector(DynObjSize);
    _Pk = Matrix(n_states, n_states);
    _Rk = Matrix(n_states, n_states);
    _Ak = Matrix(n_states,n_states);
    _Ak = 0.0;
    for (int i = 1;i<=n_states;i++)
      _Ak(i,i) = 1;
    _Bk = Matrix(n_states, _N_U * DynObjSize);
    _Qk = Matrix(n_states, n_states);
    _Uk = ColumnVector(_N_U * DynObjSize);
    //MIRAR EL TEMA DE LOS INDICES
    //    int n1 = Convert.ToInt32(Convert.ToString(RobotID[7]));
    //    int n2 = Convert.ToInt32(Convert.ToString(RobotID[8]));
    //    double Obj_ID = (double)(n1 * 10000 + n2 * 1000);
    //Initializae Xk
    for (int i = 0; i < DynObjSize; i++)
    {
      _Xk(i * _N_VAR + 1) = DynObjs.List[i].Pos3D.f_x;
      _Xk(i * _N_VAR + 2) = DynObjs.List[i].Pos3D.f_x;
      _Xk(i * _N_VAR + 3) = DynObjs.List[i].Pos3D.f_x;
      _Xk(i * _N_VAR + 4) = DynObjs.List[i].PhysCharac.f_PhysSize;
      _Xk(i * _N_VAR + 5) = 0;// DynObjs.List[i].PhysCharac.e_ObjectColor;  //R from RGB
      _Xk(i * _N_VAR + 6) = 0;//DynObjs.List[i].PhysCharac.e_ObjectColor;  //G from RGB
      _Xk(i * _N_VAR + 7) = 0;//DynObjs.List[i].PhysCharac.e_ObjectColor;  //B from RGB
      _Xk(i * _N_VAR + 8) = 0; // Temperatura
      //Object ID
      //MIRAR EL TEMA DE LOS INDICES
      //_ObjectID(i+1,1) = (Obj_ID+1);
      //Obj_ID++;
    }
    // _maxID = Obj_ID;

    for (int i = 1; i <= n_states; i++)
    {
      for (int j = 1; j <= n_states; j++)
      {
        if (i == j)
        {
          switch (n_aux1)
          {
            case 1:
            case 2: _Pk(i, j) = _cP_Pos; _Qk(i, j) = _cQ_Pos; _Rk(i, j) = _cR_Pos; break;
            case 3: _Pk(i, j) = _cP_Size; _Qk(i, j) = _cQ_Size; _Rk(i, j) = _cR_Size; break;
            case 4:
            case 5:
            case 6: _Pk(i, j) = _cP_Color; _Qk(i, j) = _cQ_Color; _Rk(i, j) = _cR_Color; break;
            case 7: _Pk(i, j) = _cP_Temp; _Qk(i, j) = _cQ_Temp; _Rk(i, j) = _cR_Temp; break;
            default: break;
          }
          n_aux1++;
          if (n_aux1 == _N_VAR + 1)
            n_aux1 = 1;
        }

      }
      for (int j = 1; j <= _N_U * DynObjSize; j++)
      {
        switch (i - n_aux2 * _N_VAR)
        {
          case 1:
            if (j == (1 + n_aux2 * _N_U))
            {
              _Bk(i, j) = _INC_TIME;
            }
            break;
          case 2:
            if (j == (2 + n_aux2 * _N_U))
            {
              _Bk(i, j) = _INC_TIME;
            }
            break;
          case 3:
            if (j == (3 + n_aux2 * _N_U))
            {
              _Bk(i, j) = _INC_TIME;
            }
            break;
          default:
            _Bk(i, j) = 0; break;

        }
        if (first)
        {
          _Uk(j) = 0;
        }
      }
      if (i - n_aux2 * _N_VAR >= _N_VAR)
        n_aux2++;
    }
    first = false;
    _Xk_ant = _Xk;
  }

  /* ------------------------------------------------------------------------- */
  /**     \fn public ComputePrediction()
   *
   *  \brief Compute the prediction based on the state or the covariance matrix
   */
  /* ------------------------------------------------------------------------- */
  void ComputePrediction()
  {

    _Xpred = _Ak * _Xk + _Bk * _Uk;
    _Ppred = _Ak * _Pk * _Ak.t() + _Qk;

  }


  /* ------------------------------------------------------------------------- */
  /**     \fn public Matrix Pairing(cl_DynamicObjects DynObjs)
   *
   *  \brief Pairing between the measurement DynObjs and the state vector
   *  \param[in] cl_DynamicObjects DynObjs
   *  \param[out] Matrix Selected Pairs
   *
   */
  /* ------------------------------------------------------------------------- */

  ReturnMatrix Pairing(cl_DynamicObjects DynObjs)
  {
    ColumnVector X_sub = ColumnVector(_N_VAR);
    Matrix P_sub = Matrix(_N_VAR, _N_VAR);
    ColumnVector Z_sub = ColumnVector(_N_VAR);
    int DynObjsSize = DynObjs.List.size();
    bool first = true;
    _Zk.CleanUp();// = ColumnVector();
    Matrix Posible_Pairs = Matrix(_N_OBJ*DynObjsSize,3);
    Matrix Sel_Pairs = Matrix(fmin(_N_OBJ,DynObjsSize),3);
    //Compute the values of the distance between all the objects
    for (int i = 1; i <= _N_OBJ; i++)
    {
      for (int k = 1; k <= _N_VAR; k++)
      {
        X_sub(k) = _Xk(k + (i - 1) * _N_VAR);
        P_sub(k, 1) = _Pk(k + (i - 1) * _N_VAR, k + (i - 1) * _N_VAR);
      }
      for (int j = 0; j < DynObjsSize; j++)
      {
        Z_sub(1) = DynObjs.List[j].Pos3D.f_x;
        Z_sub(2) = DynObjs.List[j].Pos3D.f_y;
        Z_sub(3) = DynObjs.List[j].Pos3D.f_z;
        //PRUEBA AQUI HAY QUE PONER EL TAMAÑO Y COLOR Y TEMPERATURA REAL
        if (DynObjs.List[j].Pos3D.f_x < -118)
        {
          Z_sub(4) = 5;// DynObjs.List[j].PhysCharac.f_PhysSize;
          Z_sub(5) = 255;// DynObjs.List[j].PhysCharac.e_ObjectColor;  //R
          Z_sub(6) = 0;//DynObjs.List[j].PhysCharac.e_ObjectColor;  //G
          Z_sub(7) = 0;//DynObjs.List[j].PhysCharac.e_ObjectColor;  //B
          Z_sub(8) = 28;  //TEMP
        }
        else
        {
          Z_sub(4) = 10;// DynObjs.List[j].PhysCharac.f_PhysSize;
          Z_sub(5) = 0;// DynObjs.List[j].PhysCharac.e_ObjectColor;  //R
          Z_sub(6) = 0;//DynObjs.List[j].PhysCharac.e_ObjectColor;  //G
          Z_sub(7) = 255;//DynObjs.List[j].PhysCharac.e_ObjectColor;  //B
          Z_sub(8) = 15;  //TEMP
        }

        if (first)
        {


          Matrix auxMat = Matrix(_N_VAR,1);
          auxMat = 1.0;
          auxMat.Release();
          _Zk = cl_KalAuxFun::AddRow(_Zk,auxMat); //   InsertMatrix(_Zk,auxMat,_Zk.Nrows(),1);
          //_Zk.Insert(_Zk.RowCount + 1, 1, Matrix.Ones(_N_VAR,1));
          _Zk(1 + j * _N_VAR) = DynObjs.List[j].Pos3D.f_x;
          _Zk(2 + j * _N_VAR) = DynObjs.List[j].Pos3D.f_y;
          _Zk(3 + j * _N_VAR) = DynObjs.List[j].Pos3D.f_z;
          //PRUEBAS, PONER VALOR CORRECTO
          if (DynObjs.List[j].Pos3D.f_x < -118)
          {
            _Zk(4 + j * _N_VAR) = 5;// DynObjs.List[j].PhysCharac.f_PhysSize;
            _Zk(5 + j * _N_VAR) = 255;// DynObjs.List[j].PhysCharac.e_ObjectColor;  //R
            _Zk(6 + j * _N_VAR) = 0;//DynObjs.List[j].PhysCharac.e_ObjectColor;  //G
            _Zk(7 + j * _N_VAR) = 0;//DynObjs.List[j].PhysCharac.e_ObjectColor;  //B
            _Zk(8 + j * _N_VAR) = 28;  //TEMP
          }
          else
          {
            _Zk(4 + j * _N_VAR) = 10;// DynObjs.List[j].PhysCharac.f_PhysSize;
            _Zk(5 + j * _N_VAR) = 0;// DynObjs.List[j].PhysCharac.e_ObjectColor;  //R
            _Zk(6 + j * _N_VAR) = 0;//DynObjs.List[j].PhysCharac.e_ObjectColor;  //G
            _Zk(7 + j * _N_VAR) = 255;//DynObjs.List[j].PhysCharac.e_ObjectColor;  //B
            _Zk(8 + j * _N_VAR) = 15;  //TEMP
          }
        }
        //Calculate Mahalanobis Distance
        Posible_Pairs((j + 1) + (i-1) * DynObjsSize, 1) = i;
        Posible_Pairs((j + 1) + (i-1) * DynObjsSize, 2) = j;
        Posible_Pairs((j + 1) + (i-1) * DynObjsSize, 3) = MahalanobisDist(X_sub,Z_sub,P_sub);
      }
      first = false;
    }
    //Find the minimum distance pairs

    Sel_Pairs(1,1) = Posible_Pairs(1,1);
    Sel_Pairs(1,2) = Posible_Pairs(1,2);
    Sel_Pairs(1,3) = Posible_Pairs(1,3);
    for(int i = 1;i<=fmin(_N_OBJ,DynObjsSize);i++)
    {
      for(int j = 1;j<=(_N_OBJ*DynObjsSize);j++)
      {
        if ((Sel_Pairs(i,3) > Posible_Pairs(j,3) && Posible_Pairs(j,3)>0) || Sel_Pairs(i,3) == -1)
        {
          Sel_Pairs(i,1) = Posible_Pairs(j,1);
          Sel_Pairs(i,2) = Posible_Pairs(j,2);
          Sel_Pairs(i,3) = Posible_Pairs(j,3);
        }
      }
      for (int j = 1;j<=_N_OBJ*DynObjsSize;j++)
      {
        if (Sel_Pairs(i,1) == Posible_Pairs(j,1))
          Posible_Pairs(j,3) = -1;
        if (Sel_Pairs(i,2) == Posible_Pairs(j,2))
          Posible_Pairs(j,3)= -1;

      }
      if (i + 1 <= fmin(_N_OBJ, DynObjsSize))
        Sel_Pairs(i+1,3) = -1;
    }

    //Eliminate the pairs with distances too big
    for (int i = 1; i <= Sel_Pairs.Nrows(); i++)
    {
      if(Sel_Pairs(i,3)>50)
      {
        Sel_Pairs = cl_KalAuxFun::EliminateRow(Sel_Pairs,i);
        // Sel_Pairs.ExtractRow(i);
      }
    }
    Sel_Pairs.Release();
    return Sel_Pairs;



  }

  /* ------------------------------------------------------------------------- */
  /**     \fn public MahalanobisDist(Matrix X_sub, Matrix Z_sub, Matrix P_sub)
   *
   *  \brief Calculate the Mahalanobis Distance between the one object and one measurement
   *
   *  \param[in] Matrix X_sub State Vector
   *  \param[in] matrix Z_sub Measurements
   *  \param[in] Matrix P_sub Covariances
   *  \param[out] double Mahalanobis distance
   *
   */
  /* ------------------------------------------------------------------------- */

  double MahalanobisDist(const Matrix& X_sub, const Matrix& Z_sub, const Matrix& P_sub)
  {
    double sum = 0;
    double aux1 = 0;
    double aux2 = 0;
    double MahDist = 0;
    for (int i = 1; i <= _N_VAR; i++)
    {
      aux1 = pow((X_sub(i,1)-Z_sub(i,1)),2);
      aux2 = pow(P_sub(i,1),2);
      sum = sum + (aux1 / aux2);
    }
    MahDist = sqrt(sum);
    return MahDist;

  }

  /* ------------------------------------------------------------------------- */
  /**     \fn public PrepareFilterMatrix(Matrix Sel_Pairs,cl_DynamicObjects DynObjs)
   *
   *  \brief Calculate the Mahalanobis Distance between the one object and one measurement
   *
   *  \param[in] Matrix Sel_Pairs
   *  \param[in] cl_DynamicObjects DynObjs
   *  \param[out]
   *
   */
  /* ------------------------------------------------------------------------- */

  void PrepareFilterMatrix(const Matrix& Sel_Pairs,cl_DynamicObjects DynObjs)
  {

    int i_x, i_z;
    _Xpred_sub = ColumnVector(Sel_Pairs.Nrows() * _N_VAR);
    _Ppred_sub = Matrix(Sel_Pairs.Nrows() * _N_VAR, Sel_Pairs.Nrows()* _N_VAR);
    _Zk_sub =  ColumnVector(Sel_Pairs.Nrows() * _N_VAR);
    _Hk_sub = IdentityMatrix(Sel_Pairs.Nrows() * _N_VAR);
    _Rk_sub = Matrix(Sel_Pairs.Nrows() * _N_VAR,Sel_Pairs.Nrows() * _N_VAR);
    _Rk_sub = 0.0;

    //Include the filter pairs first
    for (int k9 = 1; k9 <= Sel_Pairs.Nrows(); k9++)
    {
      i_x = (int)Sel_Pairs(k9,1);
      i_z = (int)Sel_Pairs(k9,2);
      for (int j = 1;j<= _N_VAR;j++)
      {
        _Xpred_sub(j + (k9 - 1) * _N_VAR ) = _Xpred(j + (i_x - 1) * _N_VAR);
        _Ppred_sub(j + (k9 - 1) * _N_VAR , j + (k9 - 1) * _N_VAR) = _Ppred(j + (i_x - 1) * _N_VAR, j + (i_x - 1) * _N_VAR);
        _Zk_sub(j + (k9 - 1) * _N_VAR) = _Zk(j + (i_z * _N_VAR));
        _Rk_sub(j + (k9 - 1) * _N_VAR, j + (k9 - 1) * _N_VAR) = _Rk(j + (i_z * _N_VAR), j + (i_z * _N_VAR));
      }
    }
  }

  /* ------------------------------------------------------------------------- */
  /**     \fn public ComputeFilter()
   *
   *  \brief
   *
   *  \param[in]
   *  \param[out]
   *
   */
  /* ------------------------------------------------------------------------- */

  void ComputeFilter()
  {
    IdentityMatrix Iden;
    _Xk_sub.CleanUp();// = Matrix();
    _Pk_sub.CleanUp();// = Matrix();
    _Yk_sub.CleanUp();// = Matrix();
    _Sk_sub.CleanUp();// = Matrix();
    _Kk_sub.CleanUp();// = Matrix();

    _Yk_sub = _Zk_sub - _Hk_sub * _Xpred_sub;
    _Sk_sub = _Hk_sub * _Ppred_sub * _Hk_sub.t() + _Rk_sub;
    _Kk_sub = _Ppred_sub * _Hk_sub.t() * _Sk_sub.i();

    Iden = IdentityMatrix(_Kk_sub.Ncols());
    _Xk_sub = _Xpred_sub + _Kk_sub * _Yk_sub;
    _Pk_sub = (Iden - _Kk_sub * _Hk_sub) * _Ppred_sub;
  }


  /* ------------------------------------------------------------------------- */
  /**     \fn public RepairFilterMatrix()
   *
   *  \brief
   *
   *  \param[in]
   *  \param[out]
   *
   */
  /* ------------------------------------------------------------------------- */

  void RepairFilterMatrix(const Matrix& Sel_Pairs,cl_DynamicObjects DynObjs)
  {
    int i_sub1, i_sub2,i_sub3;
    int i_x;
    ColumnVector ObjAdded;
    ColumnVector X_aux(_N_VAR);
    Matrix P_aux = Matrix(1,1);
    Matrix B_aux;
    Matrix Q_aux;
    Matrix R_aux;
    ColumnVector Time_aux;
    ColumnVector ID_aux;
    Matrix M_aux;
    ColumnVector IncTime;
    Matrix Maux1;
    Matrix Maux2;

    bool add_object = true;

    //Make copies of the real variables
    B_aux = _Bk;
    Q_aux = _Qk;
    R_aux = _Rk;
    Time_aux = _TimeDetection;
    ID_aux = _ObjectID;

    //First we include the objects computed in the filter
    if (Sel_Pairs.Nrows() > 0)
    {
      //Empty the actual variables
      _Xk.CleanUp();
      _Pk.CleanUp();
      _Bk.CleanUp();
      _Qk.CleanUp();
      _Rk.CleanUp();
      _TimeDetection.CleanUp();
      _ObjectID.CleanUp();
      _ObjectID = Matrix(Sel_Pairs.Nrows(), 1);
      //Save the recently computed values
      _Xk = _Xk_sub;
      _Pk = _Pk_sub;
      _Ak = IdentityMatrix(Sel_Pairs.Nrows() * _N_VAR);
      _TimeDetection = ColumnVector(Sel_Pairs.Nrows()); //Time since detection
      _TimeDetection = 0.0;


      for (int i = 1; i <= Sel_Pairs.Nrows(); i++)
      {
        i_x = (int)Sel_Pairs(i,1);
        i_sub1 = 1 + (i_x - 1) * _N_VAR;
        i_sub2 = _N_VAR + (i_x - 1) * _N_VAR;
        i_sub3 = 1 + (i_x - 1) * _N_U;
        Maux1 = Q_aux.SubMatrix(i_sub1, i_sub2, i_sub1, i_sub2);
        Maux2 = B_aux.SubMatrix(i_sub1, i_sub2, i_sub3, i_sub3 + 1);
        _Qk = cl_KalAuxFun::InsertMatrix(_Qk,Q_aux.SubMatrix(i_sub1, i_sub2, i_sub1, i_sub2),1 + (i - 1) * _N_VAR,1 + (i - 1) * _N_VAR);
        _Bk = cl_KalAuxFun::InsertMatrix(_Bk,B_aux.SubMatrix(i_sub1, i_sub2, i_sub3, i_sub3 + 1),1 + (i - 1) * _N_VAR, 1 + (i - 1) * _N_U);
        _Rk = cl_KalAuxFun::InsertMatrix(_Rk,R_aux.SubMatrix(i_sub1, i_sub2, i_sub1, i_sub2),1 + (i - 1) * _N_VAR, 1 + (i - 1) * _N_VAR);
        //      _Qk.Insert(1 + (i - 1) * _N_VAR, 1 + (i - 1) * _N_VAR, Q_aux.Extract(i_sub1, i_sub2, i_sub1, i_sub2));
        //      _Bk.Insert(1 + (i - 1) * _N_VAR, 1 + (i - 1) * _N_U, B_aux.Extract(i_sub1, i_sub2, i_sub3, i_sub3 + 1));
        //      _Rk.Insert(1 + (i - 1) * _N_VAR, 1 + (i - 1) * _N_VAR, R_aux.Extract(i_sub1, i_sub2, i_sub1, i_sub2));
        _ObjectID(i) = ID_aux(i_x);
      }
    }
    // Insert old objects not seen this time
    ObjAdded = Sel_Pairs.Column(1);
    _ObjNotDetected.CleanUp();
    IncTime.CleanUp();
    IncTime = Matrix(1, 1);
    IncTime(1) = _INC_TIME;
    if (_Xpred.Nrows() / _N_VAR > Sel_Pairs.Nrows())
    {

      for (int i = 1; i <= _Xpred.Nrows() / _N_VAR; i++)
      {
        add_object = true;
        for (int j = 1; j <= ObjAdded.Nrows(); j++)
        {
          if (i == ObjAdded(j))
          {
            add_object = false;
            break;
          }
        }
        //Check if the object has not been seen in a too much time
        // M_aux = Time_aux.Extract(i, i, 1, 1);
        if(Time_aux(i)>3)//  if (M_aux[1, 1].Re > 3)  //si el objeto no se ha visto mas de 3 segundos
        {
          add_object = false;
        }
        if (add_object)
        {
          _ObjNotDetected = cl_KalAuxFun::AddRow(_ObjNotDetected,(float)i);
          // _ObjNotDetected.Insert(_ObjNotDetected.RowCount + 1, 1, i * Matrix.Identity(1));

          i_sub1 = (i - 1) * _N_VAR + 1;
          i_sub2 = (i - 1) * _N_VAR + _N_VAR;
          i_sub3 = 1 + (i - 1) * _N_U;
          _Xk = cl_KalAuxFun::AddRow(_Xk,_Xpred.SubMatrix(i_sub1,i_sub2 , 1, 1));
          // _Xk.Insert(_Xk.RowCount + 1, 1, _Xpred.Extract(i_sub1,i_sub2 , 1, 1));
          _Pk = cl_KalAuxFun::InsertMatrix(_Pk,_Ppred.SubMatrix(i_sub1, i_sub2, i_sub1, i_sub2),_Pk.Nrows()+1,_Pk.Ncols()+1);
          // _Pk.Insert(_Pk.RowCount+1, _Pk.ColumnCount+1, _Ppred.Extract(i_sub1, i_sub2, i_sub1, i_sub2));
          _Ak = cl_KalAuxFun::InsertMatrix(_Ak,IdentityMatrix(_N_VAR),_Ak.Nrows()+1,_Ak.Ncols()+1);
          //_Ak.Insert(_Ak.RowCount+1, _Ak.ColumnCount+1, Matrix.Identity(_N_VAR));
          _Qk = cl_KalAuxFun::InsertMatrix(_Qk,Q_aux.SubMatrix(i_sub1,i_sub2,i_sub1,i_sub2),_Qk.Nrows()+1,_Qk.Ncols()+1);
          // _Qk.Insert(_Qk.RowCount+1,_Qk.ColumnCount+1,Q_aux.Extract(i_sub1,i_sub2,i_sub1,i_sub2));
          _Rk = cl_KalAuxFun::InsertMatrix(_Rk,R_aux.SubMatrix(i_sub1, i_sub2, i_sub1, i_sub2),_Rk.Nrows()+1,_Rk.Ncols()+1);
          // _Rk.Insert(_Rk.RowCount + 1, _Rk.ColumnCount + 1, R_aux.Extract(i_sub1, i_sub2, i_sub1, i_sub2));
          _Bk = cl_KalAuxFun::InsertMatrix(_Bk,B_aux.SubMatrix(i_sub1,i_sub2,i_sub3,i_sub3+1),_Bk.Nrows()+1,_Bk.Ncols()+1);
          // _Bk.Insert(_Bk.RowCount+1,_Bk.ColumnCount+1,B_aux.Extract(i_sub1,i_sub2,i_sub3,i_sub3+1));

          _TimeDetection = cl_KalAuxFun::AddRow(_TimeDetection,Time_aux(i));
          //_TimeDetection.Insert(_TimeDetection.RowCount+1,1, Time_aux.Extract(i,i,1,1) + IncTime);
          _ObjectID = cl_KalAuxFun::AddRow(_ObjectID,ID_aux(i));
          //_ObjectID.Insert(_ObjectID.RowCount + 1, 1, ID_aux.Extract(i, i, 1, 1));
        }
      }
    }

    //Insert new objects, if any
    ObjAdded = Sel_Pairs.Column(2);
    //Create P_aux and Q_aux and R_aux and B_aux with standard valeus for P and Q and R and B
    P_aux.CleanUp();
    P_aux = _cP_Pos * IdentityMatrix(3);
    P_aux = cl_KalAuxFun::InsertMatrix(P_aux,_cP_Size * IdentityMatrix(1),P_aux.Nrows()+1,P_aux.Ncols()+1);
    //P_aux.Insert(P_aux.RowCount + 1, P_aux.ColumnCount + 1, _cP_Size * Matrix.Identity(1));
    P_aux = cl_KalAuxFun::InsertMatrix(P_aux,_cP_Color * IdentityMatrix(3),P_aux.Nrows()+1,P_aux.Ncols()+1);
    // P_aux.Insert(P_aux.RowCount + 1, P_aux.ColumnCount + 1, _cP_Color * Matrix.Identity(3));
    P_aux = cl_KalAuxFun::InsertMatrix(P_aux,_cP_Temp * IdentityMatrix(1),P_aux.Nrows()+1,P_aux.Ncols()+1);
    // P_aux.Insert(P_aux.RowCount + 1, P_aux.ColumnCount + 1, _cP_Temp * Matrix.Identity(1));
    Q_aux.CleanUp();
    Q_aux = _cQ_Pos * IdentityMatrix(3);
    Q_aux = cl_KalAuxFun::InsertMatrix(Q_aux,_cQ_Size * IdentityMatrix(1),Q_aux.Nrows()+1,Q_aux.Ncols()+1);
    // Q_aux.Insert(Q_aux.RowCount + 1, Q_aux.ColumnCount + 1, _cQ_Size * Matrix.Identity(1));
    Q_aux = cl_KalAuxFun::InsertMatrix(Q_aux,_cQ_Color * IdentityMatrix(3),Q_aux.Nrows()+1,Q_aux.Ncols()+1);
    //Q_aux.Insert(Q_aux.RowCount + 1, Q_aux.ColumnCount + 1, _cQ_Color * Matrix.Identity(3));
    Q_aux = cl_KalAuxFun::InsertMatrix(Q_aux,_cQ_Temp * IdentityMatrix(1),Q_aux.Nrows()+1,Q_aux.Ncols()+1);
    //Q_aux.Insert(Q_aux.RowCount + 1, Q_aux.ColumnCount + 1, _cQ_Temp * Matrix.Identity(1));
    R_aux.CleanUp();
    R_aux = _cR_Pos * IdentityMatrix(3);
    R_aux = cl_KalAuxFun::InsertMatrix(R_aux,_cR_Size * IdentityMatrix(1),R_aux.Nrows()+1,R_aux.Ncols()+1);
    //R_aux.Insert(R_aux.RowCount + 1, R_aux.ColumnCount + 1, _cR_Size * Matrix.Identity(1));
    R_aux = cl_KalAuxFun::InsertMatrix(R_aux,_cR_Color * IdentityMatrix(3),R_aux.Nrows()+1,R_aux.Ncols()+1);
    // R_aux.Insert(R_aux.RowCount + 1, R_aux.ColumnCount + 1, _cR_Color * Matrix.Identity(3));
    R_aux = cl_KalAuxFun::InsertMatrix(R_aux,_cR_Temp * IdentityMatrix(1),R_aux.Nrows()+1,R_aux.Ncols()+1);
    //R_aux.Insert(R_aux.RowCount + 1, R_aux.ColumnCount + 1, _cR_Temp * Matrix.Identity(1));
    B_aux.CleanUp();
    B_aux = _INC_TIME * IdentityMatrix(_N_U);
    Matrix MatAux(_N_VAR-_N_U,_N_U);
    MatAux = 0.0;
    MatAux.ReleaseAndDelete();
    B_aux = cl_KalAuxFun::AddRow(B_aux,MatAux);
    //B_aux.Insert(B_aux.RowCount + 1, 1, Matrix.Zeros(_N_VAR-_N_U,_N_U));
    _NewObjDetected.CleanUp();// = new Matrix();
    if (Sel_Pairs.Nrows() < (int)DynObjs.List.size())
    {
      add_object = true;
      for (int i = 0; i < (int)DynObjs.List.size(); i++)
      {
        for (int j = 1; j <= ObjAdded.Nrows(); j++)
        {
          if (i == ObjAdded(j))
          {
            add_object = false;
            break;
          }
        }
        if (add_object)
        {
          _NewObjDetected = cl_KalAuxFun::AddRow(_NewObjDetected,(i + 1));//.Insert(_NewObjDetected.RowCount + 1, 1, (i + 1) * Matrix.Identity(1));
          _Xk = cl_KalAuxFun::AddRow(_Xk,_Zk.SubMatrix(i+1,i+1+8,1,1));
          // _Xk.Insert(_Xk.RowCount + 1, 1, _Zk.Column(i+1));
          _Pk = cl_KalAuxFun::InsertMatrix(_Pk,P_aux,_Pk.Nrows(),_Pk.Ncols());
          //_Pk.Insert(_Pk.RowCount + 1, _Pk.ColumnCount + 1, P_aux);
          _Ak = cl_KalAuxFun::InsertMatrix(_Ak,IdentityMatrix(_N_VAR),_Ak.Nrows()+1,_Ak.Ncols()+1);
          // _Ak.Insert(_Ak.RowCount + 1, _Ak.ColumnCount + 1, Matrix.Identity(_N_VAR));
          _Qk = cl_KalAuxFun::InsertMatrix(_Qk,Q_aux,_Qk.Nrows()+1,_Qk.Ncols()+1);
          //_Qk.Insert(_Qk.RowCount + 1, _Qk.ColumnCount + 1, Q_aux);
          _Bk = cl_KalAuxFun::InsertMatrix(_Bk,B_aux,_Bk.Nrows()+1,_Bk.Ncols()+1);
          //_Bk.Insert(_Bk.RowCount + 1, _Bk.ColumnCount + 1, B_aux);
          _Rk = cl_KalAuxFun::InsertMatrix(_Rk,R_aux,_Rk.Nrows()+1,_Rk.Ncols()+1);
          //_Rk.Insert(_Rk.RowCount + 1, _Rk.ColumnCount + 1, R_aux);
          _TimeDetection = cl_KalAuxFun::AddRow(_TimeDetection,0);
          //_TimeDetection.Insert(_TimeDetection.RowCount + 1, 1, Matrix.Zeros(1,1));
          //        M_aux = new Matrix(1, 1);
          //        M_aux = M
          //        M_aux[1,1].Re = (double)(_maxID + 1);
          _maxID++;
          _ObjectID = cl_KalAuxFun::AddRow(_ObjectID,_maxID);//.Insert(_ObjectID.RowCount + 1, 1, M_aux);
        }
      }
    }

  }


  /* ------------------------------------------------------------------------- */
  /**     \fn public ComputeU()
   *
   *  \brief
   *
   *  \param[in]
   *  \param[out]
   *
   */
  /* ------------------------------------------------------------------------- */
  void ComputeU(const Matrix& Sel_Pairs)
  {
    _Uk.CleanUp();
    _Uk = ColumnVector(3 * _Xk.Nrows() / _N_VAR);
    int i = 1;
    while(i<=_Xk.Nrows()/_N_VAR)
    {
      if (i <= Sel_Pairs.Nrows())
      {
        _Uk(1 + (i - 1) * _N_U) = (_Xk(1 + (i - 1) * _N_VAR) - _Xk_ant(1 + (int)(Sel_Pairs(i,1) - 1) * _N_VAR))/_INC_TIME;
        _Uk(2 + (i - 1) * _N_U) = (_Xk(2 + (i - 1) * _N_VAR) - _Xk_ant(2 + (int)(Sel_Pairs(i,1) - 1) * _N_VAR))/_INC_TIME;
        _Uk(3 + (i - 1) * _N_U) = (_Xk(3 + (i - 1) * _N_VAR) - _Xk_ant(3 + (int)(Sel_Pairs(i,1) - 1) * _N_VAR))/_INC_TIME;
        i++;
      }
      else
      {
        for (int j = 1; j <= _ObjNotDetected.Nrows(); j++)
        {
          _Uk(1 + (i - 1) * _N_U) = (_Xk(1 + (i - 1) * _N_VAR) - _Xk_ant(1 + (int)(_ObjNotDetected(j) - 1) * _N_VAR)) / _INC_TIME;
          _Uk(2 + (i - 1) * _N_U) = (_Xk(2 + (i - 1) * _N_VAR) - _Xk_ant(2 + (int)(_ObjNotDetected(j) - 1) * _N_VAR)) / _INC_TIME;
          _Uk(3 + (i - 1) * _N_U) = (_Xk(3 + (i - 1) * _N_VAR) - _Xk_ant(3 + (int)(_ObjNotDetected(j) - 1) * _N_VAR)) / _INC_TIME;
          i++;
        }
        for (int j = 1; j <= _NewObjDetected.Nrows(); j++)
        {
          _Uk(1 + (i - 1) * _N_U) = 0;
          _Uk(2 + (i - 1) * _N_U) = 0;
          _Uk(3 + (i - 1) * _N_U) = 0;
          i++;
        }
      }
    }
    _Xk_ant.CleanUp();// = new Matrix();
    _Xk_ant = _Xk;
  }
  /* ------------------------------------------------------------------------- */
  /**     \fn public ComputeIncTime()
   *
   *  \brief
   *
   *  \param[in]
   *  \param[out]
   *
   */
  /* ------------------------------------------------------------------------- */
  void ComputeIncTime(float actTime)
  {
    _INC_TIME = actTime - _LastTime;
    _LastTime = actTime;
  }



/* ------------------------------------------------------------------------- */
/**     \fn public KalmanFilterMain(cl_DynamicObjects DynObjs,float actTime)
 *
 *  \brief
 *
 *  \param[in] cl_DynamicObjects DynObjs
 *  \param[in] float actTime
 *  \param[out]
 *
 */
/* ------------------------------------------------------------------------- */

static cl_DynamicObjects KalmanFilterMain(cl_DynamicObjects DynObjs, float actTime, float RobotID)
{
  Matrix Sel_Pairs;
  cl_DynamicObjects DynObjects_result;// = cl_DynamicObjects2();
  if ( _first)
  {
    InitializeFilter(DynObjs,RobotID);
    ComputeIncTime(actTime);
    _first = false;
  }
  else
  {
    ComputeIncTime(actTime);
    ComputePrediction();
    if (DynObjs.List.size() > 0)
    {
      Sel_Pairs = Pairing(DynObjs);
      if (Sel_Pairs.Nrows() > 0)
      {
        PrepareFilterMatrix(Sel_Pairs, DynObjs);
        ComputeFilter();
      }
      else
      {
        Sel_Pairs.CleanUp();// = new Matrix();
      }
    }
    else
    {
      Sel_Pairs.CleanUp();// = new Matrix();
    }
    RepairFilterMatrix(Sel_Pairs, DynObjs);
    ComputeU(Sel_Pairs);

  }
  _N_OBJ = _Xk.Nrows() / _N_VAR;
  //Change back to Dynamic Object Structure
  DynObjects_result = State2DynObjList(actTime);
  return DynObjects_result;
}


/* ------------------------------------------------------------------------- */
/**     \fn public State2DynObjList()
 *
 *  \brief
 *
 *  \param[in]
 *  \param[out]
 *
 */
/* ------------------------------------------------------------------------- */
cl_DynamicObjects State2DynObjList(float actTime)
{
//Create Dynamic Object List at the end of each Iteration
cl_DynamicObjects DynamicObjects = cl_DynamicObjects((int)_TimeDetection.Nrows());
for (int i = 0; i < _TimeDetection.Nrows(); i++)
{
 // DynamicObjects.List[i].r_DetectionTime = (float)(actTime - (_TimeDetection(i+1)));
  DynamicObjects.List[i].PhysCharac.f_PhysSize = (float)_Xk(4 + i * _N_VAR);
  DynamicObjects.List[i].Pos3D.f_x = _Xk(1 + i * _N_VAR);
  DynamicObjects.List[i].Pos3D.f_y = _Xk(2 + i * _N_VAR);
  DynamicObjects.List[i].Pos3D.f_z = _Xk(3 + i * _N_VAR);
  DynamicObjects.List[i].Vel3D.d_vx = _Uk(1 + i * _N_U);
  DynamicObjects.List[i].Vel3D.d_vy = _Uk(2 + i * _N_U);
  DynamicObjects.List[i].Vel3D.d_vz = _Uk(3 + i * _N_U);
  DynamicObjects.List[i].f_Identification= (float)_ObjectID(i+1);
}
return DynamicObjects;



};
}
